/*
 * GazeboClient.cpp
 *
 *  Created on: 2011-03-27
 *      Author: kamil
 */

#include "GazeboClient.h"
#include <string>
#include "mgr/config/Config.h"

log4cxx::LoggerPtr GazeboClient::logger(log4cxx::Logger::getLogger("GazeboClient"));



GazeboClient::GazeboClient() : client(0), simIface(0) {
	LOG4CXX_INFO(logger, "Connecting to gazebo...");
	client = new libgazebo::Client();
	simIface = new libgazebo::SimulationIface();
	int serverId = 0;
	std::string id("default");
	try {
		/// Connect to the libgazebo server
//		client->ConnectWait(serverId, GZ_CLIENT_ID_USER_FIRST+0x01);
		client->Connect(serverId);
		/// Open the Simulation Interface
		simIface->Open(client, id);
	} catch (std::string e) {
		LOG4CXX_ERROR(logger, "Gazebo error: Unable to connect to the sim interface");
		exit(0);
	}
	LOG4CXX_INFO(logger, "Connected to gazebo");
}

GazeboClient::~GazeboClient() {
	if (simIface){
		simIface->Destroy();
		delete simIface;
	}

	if (client){
		client->Disconnect();
		delete client;
	}
}

void GazeboClient::setBallPosition(double x, double y, double z){
	simIface->Lock(1);
	std::string ballModel = Config::getBallInfo().name;
	libgazebo::SimulationRequestData * req = & simIface->data->requests[simIface->data->requestCount++];
	memset(req, 0, sizeof(libgazebo::SimulationRequestData));	//setting all req params to 0
	req->type = libgazebo::SimulationRequestData::SET_STATE;	//this will also stop the ball
	memcpy(req->name,ballModel.c_str(), 512);
	req->modelPose.pos.x = x;
	req->modelPose.pos.y = y;
	req->modelPose.pos.z = z;
	simIface->Unlock();
}
